package novemberBot.utils;
import novemberBot.FactoryFloorRobot;
import novemberBot.navigation.Odometer;
import novemberBot.sensor.LineChecker;
import novemberBot.sensor.UsDataResponder;
import lejos.nxt.LCD;
import lejos.util.Timer;
import lejos.util.TimerListener;

/**prints relevant information to the LCD screen
 * @author Antoine Lennartz*/
public class LCDInfo implements TimerListener{
	public static final int LCD_REFRESH = 50;
	/**link to odometer for getting position */
	private Odometer odo;
	/**link to timer for refresh*/
	private Timer lcdTimer;
	private boolean pause=false;
	/**line checker*/
	private LineChecker left, right;
	
	/**array for odo position*/
	private double [] pos;
	/**constructor*/
	public LCDInfo() {
		this.odo = FactoryFloorRobot.getInstance().getOdometer();
		this.lcdTimer = new Timer(LCD_REFRESH, this);
		left=FactoryFloorRobot.getInstance().getLeftChecker();
		right=FactoryFloorRobot.getInstance().getRightChecker();
		// initialise the arrays for displaying data
		pos = new double [3];
		
		sensorData();//setup responders
		
		// start the timer
		lcdTimer.start();
	}
	
	public void pause(){pause=true;}
	public void resume(){ pause=false;}
	/**truncates a double by casting to and from int
	 * @param d - double to be truncated
	 * @param decimalPoints - amount of points to keep
	 * @return truncated double*/
	public static double truncDouble(double d, int decimalPoints){
		double fact=Math.pow(10.0, decimalPoints);
		return ((double)((int)(d*fact)))/fact;
	}
	
	public void timedOut() { 
		if(pause) return;
		//printOdometry();
		printSensorData();
		//printLineInfo();
		

	}
	public UsDataResponder high;
	public UsDataResponder low;
	/**sets up usData responder for printing us readings*/
	public void sensorData(){
		high = new UsDataResponder("nofilter", UsDataResponder.Sensor.HIGH);
		low = new UsDataResponder("nofilter", UsDataResponder.Sensor.LOW);
		FactoryFloorRobot.getInstance().getUsPoller().addResponder(high);
		FactoryFloorRobot.getInstance().getUsPoller().addResponder(low);
		high.needsData=true;
		low.needsData=true;	
	}
	/**prints out u filtered Us Sensor data*/
	public void printSensorData(){
		LCD.clear();
		LCD.drawString("high: "+high.getFilter().getData(), 0, 0);
		LCD.drawString("low: "+low.getFilter().getData(), 0, 1);
		LCD.refresh();
		try {
			Thread.sleep(40);
		} catch (InterruptedException e) {;
		}
	}
	/**prints out position and heading*/
	public void printOdometry(){
		odo.getPosition(pos);		
		LCD.clear();
		LCD.drawString("X: ", 0, 0);
		LCD.drawString("Y: ", 0, 1);
		LCD.drawString("H: ", 0, 2);
		LCD.drawString( ""+truncDouble(pos[0], 2), 4, 0);
		LCD.drawString( ""+truncDouble(pos[1], 2), 4, 1);
		LCD.drawString( ""+truncDouble(pos[2], 2), 4, 2);
		LCD.refresh();
	}
	/**prints out "on" or "off" depending if the light
	 * sensors are currently on a line or not*/
	public void printLineInfo(){
		String leftStatus, rightStatus;
		if(left.onLine()) leftStatus="on";
		else leftStatus="off";
		if(right.onLine()) rightStatus="on";
		else rightStatus="off";
		LCD.clear();
		LCD.drawString("l: "+leftStatus, 0, 3);
		LCD.drawString("r: "+rightStatus, 0, 4);
		LCD.refresh();
	}
}
